#!/usr/bin/env python
"""
机器人遥控节点

该节点提供两种方式控制机器人底盘运动:
1. 键盘遥控: 类似于ROS标准的 'teleop_twist_keyboard'，通过键盘按键
   (w/s/a/d) 直接控制机器人的线速度和角速度。
2. 指令订阅: 订阅 'robot_command' 话题，接收字符串指令 (如 "前进", "后退"),
   并将其转换为对应的速度指令。

该脚本修改自 'teleop_twist_keyboard'，保留了其键盘控制功能，并扩展了
对ROS话题指令的支持。
"""
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy

from geometry_msgs.msg import Twist
from std_msgs.msg import String

import sys, select, termios, tty

# 节点启动时显示的帮助信息
msg = """
通过键盘或话题指令控制机器人!
---------------------------
键盘控制:
  w/s : 增加/减少线速度 (步进 0.2)
  a/d : 增加/减少角速度 (步进 0.1)
  x   : 速度归零
---------------------------
话题指令 ('robot_command'):
  "前进", "后退", "左转", "右转", "停止"
---------------------------
CTRL-C 退出
"""

# 按键与速度增量的映射关系
moveBindings = {
        'w':(0.2,0,0,0),    # 前进
        's':(-0.2,0,0,0),   # 后退
        'a':(0,0,0,0.1),    # 左转
        'd':(0,0,0,-0.1),   # 右转
           }

# 功能键与速度设定的映射关系
speedBindings={
        'x':(0,0,0,0),      # 停止
          }

def getKey():
    """
    从终端获取单个按键，无需等待回车。
    """
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key


def vels(speed,turn):
    """
    格式化并返回当前速度信息的字符串。
    """
    return "当前速度:\t线速度 %s\t角速度 %s " % (speed,turn)

def command_callback(data):
    """
    处理来自 'robot_command' 话题的指令的回调函数。

    Args:
        data (std_msgs.msg.String): 包含控制指令的ROS消息。
    """
    # 使用 global 关键字来修改在主程序中定义的全局速度变量
    global x, y, z, th
    command = data.data.lower()
    
    # 根据指令内容更新速度变量
    if command == "前进":
        x += moveBindings['w'][0]
    elif command == "后退":
        x += moveBindings['s'][0]
    elif command == "左转":
        th += moveBindings['a'][3]
    elif command == "右转":
        th += moveBindings['d'][3]
    elif command == "停止":
        x, y, z, th = 0, 0, 0, 0
    else:
        rospy.logwarn("接收到未知机器人指令: %s", command)
        return # 未知指令不执行任何操作

    rospy.loginfo("收到指令: '%s' | %s" % (command, vels(x, th)))
        
    # 创建并发布Twist消息
    twist = Twist()
    twist.linear.x = x; twist.linear.y = y; twist.linear.z = z
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th
    pub.publish(twist)

if __name__=="__main__":
    # 保存终端的原始设置，以便退出时恢复
    settings = termios.tcgetattr(sys.stdin)
    
    # 初始化ROS节点
    rospy.init_node('robot_control_node')
    
    # 创建发布器，向 '/cmd_vel_mux/input/navi' 发布Twist消息
    pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size = 1)
    
    # 创建订阅者，订阅 'robot_command' 话题，并指定回调函数
    rospy.Subscriber('robot_command', String, command_callback)
    
    # 定义全局速度变量，用于在键盘控制和指令回调之间共享状态
    # x: 线速度, th: 角速度
    global x, y, z, th
    x, y, z, th = 0, 0, 0, 0

    try:
        # 打印帮助信息
        print(msg)
        print(vels(x, th))
        
        # 主循环，用于处理键盘输入
        while(1):
            key = getKey()
            # 根据按键更新速度
            if key in moveBindings.keys():
                x += moveBindings[key][0]
                th += moveBindings[key][3]
            elif key in speedBindings.keys():
                x, y, z, th = 0, 0, 0, 0
            # 如果是 Ctrl+C (ASCII码为\x03)，则退出循环
            elif (key == '\x03'):
                break
            else:
                # 忽略其他按键
                continue

            # 打印当前速度并发布Twist消息
            print(vels(x,th))
            twist = Twist()
            twist.linear.x = x; twist.linear.y = y; twist.linear.z = z;
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th;
            pub.publish(twist)

    except Exception as e:
        print(e)

    finally:
        # 在程序退出前，发布一个零速度的Twist消息，确保机器人停止
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

        # 恢复终端的原始设置
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)